﻿#pragma once
#include <cmath>
#include "point2.h"
#include "point3.h"

#ifndef MMO_PI
#define MMO_PI 	(3.14159265358979323846264338327950288f)
#define MMO_PI2 (6.2831853f)			//MMO_PI * 2
#define MMO_PI8 (25.1327412f)			//MMO_PI * 8
#define MMO_PI_2 (1.570796325f)			//MMO_PI / 2	180
#define MMO_PI_3 (1.0471975512f)		//MMO_PI / 3	120
#define MMO_PI_4 (0.7853981625f)		//MMO_PI / 4	90
#define MMO_PI_6 (0.5235987756f)		//MMO_PI / 6	60
#endif

#ifndef EPS
#define EPS 0.000001f
#endif

#ifndef Epsilon
#define Epsilon 0.000000001
#endif // !Epsilon

inline float AngleToRadian(float angle) 
{
    return (angle * MMO_PI / 180.0f);
}

inline float RadianToAngle(float radian) 
{
    return (radian * 180.0f / MMO_PI);
}

inline double Distance2Dpow2(Point2<float>& from, Point2<float>& to)
{
   // Point2<float> distance = to - from;
	return pow((to.x - from.x), 2) + pow((to.x - from.x), 2);
}

inline float Distance2D(Point2<float>& from, Point2<float>& to)
{
   // Point2<float> distance = to - from;
   // return sqrt(pow(distance.x, 2) + pow(distance.y, 2));
	return sqrt(pow((to.x - from.x), 2) + pow((to.x - from.x), 2));
}

inline float DistanceX(Point2<float>& from, Point2<float>& to) 
{
    return to.x - from.x;
}

inline float DistanceY(Point2<float>& from, Point2<float>& to) 
{
    return to.y - from.y;
}

//��unity-z��������Ϊ0�ȣ���������ת�Ƕ�
inline float Radian2D(const Point2<float>& from, const Point2<float>& to)
{
    Point2<float> diff = to - from;
    float ret = 0.0f;
    if (fabs(diff.x) <= EPS && fabs(diff.y) <= EPS) 
	{
        ret = 0.0f;
    }
    else if (fabs(diff.x) <= EPS && fabs(diff.y) > EPS) 
	{
        if (diff.y > 0.0f) 
		{
            ret = 0.0f;
        }
        else
		{
            ret = MMO_PI;
        }
    }
    else if (fabs(diff.x) > EPS && fabs(diff.y) <= EPS) 
	{
        if (diff.x > 0.0f) 
		{
            ret = MMO_PI / 2.0f;
        }
        else
		{
            ret = MMO_PI * 3.0f / 2.0f;
        }
    }
    else if (diff.x > 0.0f && diff.y > 0.0f) 
	{
        ret = atanf(diff.x / diff.y);
    }
    else if (diff.x > 0.0f && diff.y < 0.0f) 
	{
        ret = MMO_PI - atanf(-diff.x / diff.y);
    }
    else if (diff.x < 0.0f && diff.y < 0.0f) 
	{
        ret = MMO_PI + atanf(diff.x / diff.y);
    }
    else if (diff.x < 0.0f && diff.y > 0.0f) 
	{
        ret = MMO_PI * 3.0f / 2.0f + atanf(-diff.y / diff.x);
    }
    return ret;
}

inline float Angle2D(const Point2<float>& from, const Point2<float>& to)
{
    return RadianToAngle(Radian2D(from, to));
}

inline float VectorRadian2D(const Point2<float>& from, const Point2<float>& to)
{
    return acosf((to.x * from.x + to.y * from.y) / (sqrt(pow(from.x, 2) + pow(from.y, 2)) * sqrt(pow(to.x, 2) + pow(to.y, 2))));
}

inline float VectorAngle2D(Point2<float>& from, Point2<float>& to) 
{
    return RadianToAngle(VectorRadian2D(from, to));
}

inline void FormulaPos(float dir, float length, Point2<float>* pos) 
{
    if (dir <= MMO_PI / 2.0f) 
	{
        pos->x = length * sinf(dir);
        pos->y = length * cosf(dir);
    }
    else if (dir <= MMO_PI) 
	{
        pos->x = length * sinf(MMO_PI - dir);
        pos->y = -length * cosf(MMO_PI - dir);
    }
    else if (dir <= MMO_PI * 3 / 2.0f) 
	{
        pos->x = -length * sinf(dir - MMO_PI);
        pos->y = -length * cosf(dir - MMO_PI);
    }
    else if (dir <= MMO_PI * 2.0f) 
	{
        pos->x = -length * sinf(2.0f * MMO_PI - dir);
        pos->y = length * cosf(2.0f * MMO_PI - dir);
    }
}

inline float ReviseDir(float dir) 
{
    dir = (dir <= 0.0f) ? dir + 360.0f : dir;
    if (dir > 360.0f) 
	{
        dir = fmod(dir, 360.0f);
    }
    return dir;
}

inline float FloorDir(float dir) 
{
    return floor(ReviseDir(dir));
}

inline float DeltaAngle(float current, float target) 
{
    if (current == target) 
	{
        return 0.0f;
    }
    current = ReviseDir(current);
    target = ReviseDir(target);
    return ReviseDir(target - current);
}

inline float DeltaMinAngle(float current, float target)
{
    if (current == target) 
	{
        return 0.0f;
    }
    float delta = DeltaAngle(current, target);
    float minDelta =  delta > 360.0f - delta ? 360.0f - delta : delta;
    return ReviseDir(minDelta);
}

inline float CarmackSqrt(float number)
{
	int i;
	float x2, y;
	const float threehalfs = 1.5F;

	x2 = number * 0.5F;
	y = number;
	i = *(int *)&y;				// evil floating point bit level hacking  
	i = 0x5f375a86 - (i >> 1);	// what the fuck?  
	y = *(float *)&i;
	y = y * (threehalfs - (x2 * y * y));
	y = y * (threehalfs - (x2 * y * y));
	y = y * (threehalfs - (x2 * y * y));
	return number*y;
}

inline float point2Length(const Point2<float>& Value1, const  Point2<float>& Value2)
{
	//Point2<float> distance = Value1 - Value2;
	//return CarmackSqrt(distance.x * distance.x + distance.y * distance.y);
	return CarmackSqrt((Value1.x - Value2.x) * (Value1.x - Value2.x) + (Value1.y - Value2.y) * (Value1.y - Value2.y));
}

inline float point2Length(const Point3<float>& Value1, const Point3<float>& Value2)
{
	//Point3<float> distance = Value1 - Value2;
	//return CarmackSqrt(distance.x * distance.x + distance.z * distance.z);
	return CarmackSqrt((Value1.x - Value2.x) * (Value1.x - Value2.x) + (Value1.z - Value2.z) * (Value1.z - Value2.z));
}

inline float point3Length(const Point3<float>& Value1, const Point3<float>& Value2)
{
	//Point3<float> distance = Value1 - Value2;
	//return CarmackSqrt(distance.x * distance.x + distance.y * distance.y + distance.z * distance.z);
	return CarmackSqrt((Value1.x - Value2.x) * (Value1.x - Value2.x) + (Value1.y - Value2.y) * (Value1.y - Value2.y) + (Value1.z - Value2.z) * (Value1.z - Value2.z));
}

inline float point2LengthSquare(const Point3<float>& Value1, const Point3<float>& Value2)
{
	/*Point3<float> distance = Value1 - Value2;
	return distance.x * distance.x + distance.z * distance.z;*/
	return (Value1.x - Value2.x) * (Value1.x - Value2.x) + (Value1.z - Value2.z) * (Value1.z - Value2.z);
}

inline float point2LengthSimple(const Point3<float>& Value1, const Point3<float>& Value2)
{
	float A = (float)(fabs(Value1.x-Value2.x));
	float B = (float)(fabs(Value1.z - Value2.z));
	return A > B ? A + B/2 : B + A/2;
}

inline float point3LengthSquare(Point3<float>& Value1, Point3<float>& Value2)
{
	//Point3<float> distance = Value1 - Value2;
	//return distance.x * distance.x + distance.y * distance.y + distance.z * distance.z;
	return (Value1.x - Value2.x) * (Value1.x - Value2.x) + (Value1.y - Value2.y) * (Value1.y - Value2.y) + (Value1.z - Value2.z) * (Value1.z - Value2.z);
}


inline float triangleArea(float ax, float az, float bx, float bz, float cx, float cz)
{
	return abs(ax * bz + bx * cz + cx * az - bx * az - cx * bz - ax * cz);
}

// ������������������ε����
inline float triangleArea(Point3<float>& a, Point3<float>& b, Point3<float>& c)
{
	float f = abs((a.x * b.z + b.x * c.z + c.x * a.z - b.x * a.z
		- c.x * b.z - a.x * c.z) / 2);
	return f;
}
/**
* �ж�p�Ƿ���abcd��ɵ��ı�����
* @return ���p���ı����ڷ���true,���򷵻�false.
*/
inline bool isInRect(Point3<float>& a, Point3<float>& b, Point3<float>& c, Point3<float>& d,Point3<float>& p)
{
	float areav = 9.0f;
	float dTriangle = triangleArea(a, b, p) + triangleArea(b, c, p)	+ triangleArea(c, d, p) + triangleArea(d, a, p);
	float dQuadrangle = triangleArea(a, b, c) + triangleArea(c, d, a);
	return abs(dTriangle - dQuadrangle) < areav;
}

/*
* 是否在圆内
* midPos:圆心坐标
* radius：半径
* pos：外部坐标（判断用的）
*/
inline bool isInCircle(const Point3<float> &midPos, float radius, Point3<float> &pos)
{
	float vx = pos.x - midPos.x;
	float vz = pos.z - midPos.z;
	return (vx * vx + vz * vz) <= ((radius + 0.5) * (radius + 0.5)); //半径扩大0.5m,允许有一定的误差范围
}

